﻿using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.FEZ;
using MLRobotic.IA.Drivers;
using System.Collections;
using MLRobotic.Asserv.Tools;
using GHIElectronics.NETMF.Hardware;
using MLRobotic.IA;

namespace MLRobotic.Asserv
{
    public class Program
    {
        public static COM comPort;

        public static class Alert
        {
            public static bool blocageRobot = false;

        }

        public static void Main()
        {
            Debug.EnableGCMessages(false);

            //PWM pwmDroitTrigo = new PWM((PWM.Pin)FEZ_Pin.PWM.Di5);
            //PWM pwmDroitHoraire = new PWM((PWM.Pin)FEZ_Pin.PWM.Di6);
            //PWM pwmGaucheTrigo = new PWM((PWM.Pin)FEZ_Pin.PWM.Di9);
            //PWM pwmGaucheHoraire = new PWM((PWM.Pin)FEZ_Pin.PWM.Di10);

            //pwmDroitTrigo.Set(10000, 0);
            //pwmGaucheTrigo.Set(10000, 0);

            //for (byte i = 0; i < 100; i++)
            //{
            //    pwmDroitHoraire.Set(10000, i);
            //    pwmGaucheHoraire.Set(10000, i);
            //    Thread.Sleep(30);
            //}
            //for (byte i = 100; i > 0; i--)
            //{
            //    pwmDroitHoraire.Set(10000, i);
            //    pwmGaucheHoraire.Set(10000, i);
            //    Thread.Sleep(30);
            //}

            //Ecran.Init();
            //Capteurs.Init();

            //DateTime dt = DateTime.Now;
            //while (true)
            //{
            //    Ecran.Process();
            //    Capteurs.Process();
            //    //if (DateTime.Now > dt)
            //    //{
            //    //    dt = DateTime.Now.AddMilliseconds(300);
            //    //    Ecran.SetCursor(0,0);
            //    //    Ecran.Print(Robot.Alertes.RobotDerriere.ToString() + " ");
            //    //    Ecran.Print(Robot.Alertes.RobotEnFace.ToString()+ "  ");
            //    //}
            //}



            //Log.Init();
            bool fin = false;
            Queue testCibles = new Queue();
            //testCibles.Enqueue(new double[] { 10 * System.Math.PI });
            //testCibles.Enqueue(new double[] { 500, 0 });
            //testCibles.Enqueue(new double[] { 20000, 6000 });
            //testCibles.Enqueue(new double[] { 0, 6000 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 20000, 0 });
            //testCibles.Enqueue(new double[] { 20000, 6000 });
            //testCibles.Enqueue(new double[] { 0, 6000 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 7000, 0 });
            //testCibles.Enqueue(new double[] { 3500, 3500 });
            //testCibles.Enqueue(new double[] { 7000, 7000 });
            //testCibles.Enqueue(new double[] { 0, 7000 });
            //testCibles.Enqueue(new double[] { 0, 9000 });
            //testCibles.Enqueue(new double[] { 7000, 9000 });
            //testCibles.Enqueue(new double[] { 0, 9000 });
            //testCibles.Enqueue(new double[] { 0, 14000 });
            //testCibles.Enqueue(new double[] { 0, 0 });

            // Deplace.TetaPos = 0.031;
            //testCibles.Enqueue(new double[] { 2000, 0 });
            //testCibles.Enqueue(new double[] { 700, 300 });
            //testCibles.Enqueue(new double[] { 0, 300 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 700, 0 });
            //testCibles.Enqueue(new double[] { 700, 300 });
            //testCibles.Enqueue(new double[] { 0, 300 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 700, 0 });
            //testCibles.Enqueue(new double[] { 700, 300 });
            //testCibles.Enqueue(new double[] { 0, 300 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 300, 550 });
            //testCibles.Enqueue(new double[] { 300, 700 });
            //testCibles.Enqueue(new double[] { 300, 1800 });


            //testCibles.Enqueue(new double[] { 10 * System.Math.PI });
            //testCibles.Enqueue(new double[] { 0 });

            //testCibles.Enqueue(new double[] { 1300, 0 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 700, 0 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 700, 0 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 700, 0 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 700, 0 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 700, 0 });
            //testCibles.Enqueue(new double[] { 0, 0 });

            //testCibles.Enqueue(new double[] { 225, 225 });
            //testCibles.Enqueue(new double[] { 700, 0 });
            //testCibles.Enqueue(new double[] { 225, 225 });
            //testCibles.Enqueue(new double[] { 450, 450 });
            //testCibles.Enqueue(new double[] { 225, 225 });
            //testCibles.Enqueue(new double[] { 0, 450 });
            //testCibles.Enqueue(new double[] { 225, 225 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 450, 0 });
            //testCibles.Enqueue(new double[] { 450, 450 });
            //testCibles.Enqueue(new double[] { 0, 450 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 450, 0 });
            //testCibles.Enqueue(new double[] { 450, 450 });
            //testCibles.Enqueue(new double[] { 0, 450 });
            //testCibles.Enqueue(new double[] { 0, 0 });
            //testCibles.Enqueue(new double[] { 450, 0 });
            //testCibles.Enqueue(new double[] { 450, 450 });
            //testCibles.Enqueue(new double[] { 0, 450 });
            //testCibles.Enqueue(new double[] { 0, 0 });


            //Debug.Print("GO GO GO !");
            //Log.Write("GO GO GO !");
            //Log.Write("GO GO GO 2 !");
            Ecran.Init();
            //Ecran.Clear();
            //Ecran.Print("X:" + Deplace.Xpos.ToString("N0"));
            //Ecran.SetCursor(0, 8);
            //Ecran.Print("Y:" + Deplace.Xpos.ToString("N0"));
            //Ecran.SetCursor(1, 0);
            //Ecran.Print("Teta:" + Deplace.TetaPos.ToString("N4"));

            //// TOREMOVE
            //for (int i = 1; i < 500; i++)
            //{
            //    Ecran.Process();
            //Thread.Sleep(4000);
            //}

            //simulateur de codeur : OFF
            //InitCodeurs();

            Deplace.Init();


            comPort = new COM("COM2");


            while (true)
            {
                // Boucle principale
                Deplace.CalculPosition();
                Deplace.DeplaceBouge();

                comPort.Process();

                //Deplace.CalculPID();
                //Ecran.Process();


                //// Pour test :
                //if (Deplace.ArriveACible && testCibles.Count > 0)
                //{
                //    double[] cible = (double[])testCibles.Dequeue();
                //    // Pour test : next cible
                //    if (cible.Length == 2)
                //    {
                //        SDCard.Write("GOTO : " + cible[0].ToString() + ", " + cible[1].ToString());
                //        Deplace.GotoXY(cible[0], cible[1], true);
                //    }
                //    else
                //    {
                //        SDCard.Write("GOTO : " + cible[0].ToString("N3"));
                //        Deplace.GotoTeta(cible[0]);
                //    }

                //    Ecran.Clear();
                //    Ecran.Print("X:" + Deplace.Xpos.ToString("N0"));
                //    Ecran.SetCursor(0, 8);
                //    Ecran.Print("Y:" + Deplace.Xpos.ToString("N0"));
                //    Ecran.SetCursor(1, 0);
                //    Ecran.Print("Teta:" + Deplace.TetaPos.ToString("N4"));
                //}
                //else if (Deplace.ArriveACible && !fin)
                //{
                //    fin = true;
                //    Thread.Sleep(2000);
                //    Deplace.CalculPosition();
                //    SDCard.Write("PosFin : " + Deplace.Xpos.ToString("N0") + ";" + Deplace.Ypos.ToString("N0") + ";" + Deplace.TetaPos.ToString("F4"));
                //    Ecran.Clear();
                //    Ecran.Print("X:" + Deplace.Xpos.ToString("N0"));
                //    Ecran.SetCursor(0, 8);
                //    Ecran.Print("Y:" + Deplace.Xpos.ToString("N0"));
                //    Ecran.SetCursor(1, 0);
                //    Ecran.Print("Teta:" + Deplace.TetaPos.ToString("N4"));
                //}
            }
        }


        //#region Gestion des codeurs
        //static System.Threading.Timer tiCod;
        //private static void InitCodeurs()
        //{
        //    tiCod = new System.Threading.Timer(new System.Threading.TimerCallback(UpdateCodeurs), null, tempo, tempo);
        //}
        //static int tempo = 100;

        //public static double coefCodD = 1;
        //public static double coefCodG = 1;


        //private static void UpdateCodeurs(object sender)
        //{
        //    Deplace.codeurDroit += Convert.ToInt32((tempo * Deplace.curvalDroit / 1000 * coefCodD).ToString());
        //    Deplace.codeurGauche += Convert.ToInt32((tempo * Deplace.curvalGauche / 1000 * coefCodG).ToString());

        //}
        //#endregion


    }
}
